//package Controller;

import lejos.nxt.*;
import lejos.robotics.proposal.DifferentialPilot;
import lejos.robotics.subsumption.*;

public class Main {
	public static void main(String [] args) throws InterruptedException{
		//TODO: waarden correct invullen
		final float leftWheelDiameter = 56; 
		final float rightWheelDiameter = 56;
		final float trackWidth = 110;
		Motor leftMotor = Motor.B;
		Motor rightMotor = Motor.C;
		
		DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, leftMotor, rightMotor, false);
		LightSensor lightSensor = new LightSensor(SensorPort.S1);
		//UltrasonicSensor ultrasonicSensor = new UltrasonicSensor(SensorPort.S2);
				
		Behavior b1 = new DriveForward(pilot);
		Behavior b2 = new LookForLine(lightSensor, pilot);
		//afstand tot muur:10 cm, tolerantie van 1 cm
		//Behavior b3 = new CorrectToParallel(10, 1, pilot, ultrasonicSensor);
		Behavior [] bArray = {b1, b2}; 
		Arbitrator arby = new Arbitrator(bArray);
		arby.start();
		
		/*LCD.drawString("ClassDef", 0, 0);
		lightSensor.calibrateHigh();
		LCD.drawString("Calibrate high", 0, 0);
		Thread.sleep(5000);
		lightSensor.calibrateLow();
		LCD.drawString("Calibrate low", 0, 0);
		Thread.sleep(5000);
		
		while(true){
			LCD.clearDisplay();
			LCD.drawString("Value", 0, 0);
			LCD.drawInt(lightSensor.readValue(), 0, 1);
			LCD.drawString("Normalizedvalue", 0, 2);
			LCD.drawInt(lightSensor.readNormalizedValue(), 0, 3);
			Thread.sleep(100);
		}*/
	}
}
